/*
 * Hovercraft.cpp
 *
 *  Created on: Feb 5, 2010
 *      Author: fmason
 */
#include "hovercraft.h"

void Hovercraft::setup()
{
	leftMotor.setMotorControlPins( LEFT_MOTOR_CONTROL_PIN_1, LEFT_MOTOR_CONTROL_PIN_2, LEFT_MOTOR_PWM_PIN );
	rightMotor.setMotorControlPins( RIGHT_MOTOR_CONTROL_PIN_1, RIGHT_MOTOR_CONTROL_PIN_2, RIGHT_MOTOR_PWM_PIN );
	//servo.setup();
	sonar.setup();
	//led.setup();
	//led.setColor(0, 0, 255 );
	//while(true);
}

void Hovercraft::setRightMotorPower( int power )
{
	/*
	int distance = getDistance();
	//sendSonarOutput( distance );
	//Serial.print( "distance: " );
	Serial.println( distance );
	if( distance < TOO_CLOSE )
	{
		//Serial.println( "breaking - too close - setRightMotorPower" );
		stop();
	}*/
	if( power == OFF_DUTY )
	{
		//Serial.println( "breaking - power == OFF_DUTY - setRightMotorPower" );
		rightMotor.stopMotor();
	}
	else
	{
		rightMotor.setMotorPower( ( power * RIGHT_MOTOR_SCALE_NUMERATOR )/RIGHT_MOTOR_SCALE_DENOMINATOR );
	}
}

void Hovercraft::setRightMotorDirection( int direction )
{
	/*
	int distance = getDistance();
	//Serial.print( "distance: " );
	//Serial.println( distance );
	if( distance < TOO_CLOSE )
	{
		//Serial.println( "breaking - too close - setRightMotorDirection" );
		stop();
	}*/
	if( rightMotor.getMotorPower() == OFF_DUTY )
	{
		//Serial.println( "breaking - rightMotor.getMotorPower() == OFF_DUTY - setRightMotorDirection" );
		rightMotor.stopMotor();
	}
	else
	{
		rightMotor.setMotorDirection( direction );
	}
}
void Hovercraft::setLeftMotorPower( int power )
{
	/*
	int distance = getDistance();
	sendSonarOutput( distance );
	//Serial.print( "distance: " );
	//Serial.println( distance );
	if( distance < TOO_CLOSE )
	{
		//Serial.println( "breaking - too close - setLeftMotorPower" );
		stop();
	}*/
	if( power == OFF_DUTY ){
		//Serial.println( "breaking - power == OFF_DUTY - setLeftMotorPower" );
		leftMotor.stopMotor();
	}
	else
	{
		leftMotor.setMotorPower( ( power * LEFT_MOTOR_SCALE_NUMERATOR )/LEFT_MOTOR_SCALE_DENOMINATOR );
	}
}

void Hovercraft::setLeftMotorDirection( int direction )
{
	/*
	int distance = getDistance();
	//Serial.print( "distance: " );
	//Serial.println( distance );
	if( ( distance < TOO_CLOSE ) )
	{
		//Serial.println( "breaking - too close - setLeftMotorDirection" );
		stop();
	}*/
	if( leftMotor.getMotorPower() == OFF_DUTY )
	{
		//Serial.println( "breaking - leftMotor.getMotorPower() == OFF_DUTY - setLeftMotorDirection" );
		leftMotor.stopMotor();
	}
	else
	{
		leftMotor.setMotorDirection( direction );
	}
}

void Hovercraft::stop()
{
	rightMotor.stopMotor();
	leftMotor.stopMotor();
}
int Hovercraft::getLeftMotorPower()
{
	return leftMotor.getMotorPower();
}
int Hovercraft::getRightMotorPower()
{
	return rightMotor.getMotorPower();
}
int Hovercraft::getTurn()
{
	return rightMotor.getMotorPower() - leftMotor.getMotorPower();
}

int Hovercraft::getDistance(int sonarPosition)
{

//	if ( sonar.getDistance() > TOO_CLOSE )
//		led.setColor( 0, 255, 0 );
//	else
//		led.setColor( 255, 0, 0);

	return sonar.getDistance(sonarPosition);
}

void Hovercraft::setLEDColor()
{
	//led.setColor( 0, getRightMotorPower(), getLeftMotorPower() );
}
void Hovercraft::printMotorInput()
{
	Serial.print( "right motor power: ");
	Serial.print( rightMotor.getMotorPower() );
	Serial.print( "left motor power: ");
	Serial.print( leftMotor.getMotorPower() );
}
